#define ACCEPT_USE_OF_DEPRECATED_PROJ_API_H
#include "colmap/retrieval/visual_index.h"
#include "colmap/retrieval/utils.h"
#include "colmap/feature/utils.h"
#include "colmap/feature/types.h"
#include "colmap/util/bitmap.h"
#include "colmap/lib/FLANN/flann.h"
#include "colmap/estimators/similarity_transform.h"
#include "colmap/optim/loransac.h"
#include "Eigen/Core"
#include "opencv2/core/eigen.hpp"
#include "proj_api.h"
#include "base.h"

colmap::retrieval::VisualIndex<> visualIndex;
Eigen::Matrix<size_t, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> index_matrix;
Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> distance_matrix;

namespace OPT
{
	extern bool bFloatDesc;
	extern int nQuery;
	extern int nOverlap;
	extern int nSpatial;
	extern float fDisSpatial;
	extern float fGPSErr;
}
int num_spatial_plus = 1000;

void LoadVobtree(string visual_index_name, bool prepare)
{
	visualIndex.Read(visual_index_name);
	if (prepare)
		visualIndex.Prepare();
}

void SaveVobtree(string visual_index_name)
{
	visualIndex.Write(visual_index_name);
}

void BuildDistanceMatrix(int imageNum, Eigen::Matrix<float, Eigen::Dynamic, 3, Eigen::RowMajor>& locationMatrix)
{
	if (num_spatial_plus < OPT::nSpatial)
		num_spatial_plus = OPT::nSpatial;
	
	if (num_spatial_plus > imageNum)
		num_spatial_plus = imageNum;

	flann::Matrix<float> locations(locationMatrix.data(), imageNum, locationMatrix.cols());
	flann::LinearIndexParams index_params;
	flann::LinearIndex<flann::L2<float>> search_index(index_params);
	search_index.buildIndex(locations);

	index_matrix.resize(imageNum, num_spatial_plus);
	flann::Matrix<size_t> indices(index_matrix.data(), imageNum, num_spatial_plus);

	distance_matrix.resize(imageNum, num_spatial_plus);
	flann::Matrix<float> distances(distance_matrix.data(), imageNum, num_spatial_plus);

	flann::SearchParams search_params(flann::FLANN_CHECKS_AUTOTUNED);
	search_params.cores = std::thread::hardware_concurrency();
	if (search_params.cores <= 0)
		search_params.cores = 1;

	search_index.knnSearch(locations, indices, distances, num_spatial_plus, search_params);
}

void IndexImage(unsigned ID, cv::Mat& keys, cv::Mat& descs)
{
	assert(keys.rows == descs.rows);
	int featureNum = keys.rows;
	colmap::FeatureKeypoints keypoints;
	keypoints.resize(featureNum);
	float* data = (float*)keys.data;
	for (int j = 0; j < featureNum; j++)
	{
		keypoints[j] = colmap::FeatureKeypoint(*data, *(data + 1), *(data + 2), *(data + 3));
		data += 8;
	}

	Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> descriptorsFloat(descs.rows, descs.cols);
	cv::cv2eigen(descs, descriptorsFloat);
	colmap::FeatureDescriptors descriptors = colmap::FeatureDescriptorsToUnsignedByte(descriptorsFloat);

	visualIndex.Add(colmap::retrieval::VisualIndex<>::IndexOptions(), ID, keypoints, descriptors);
	LOG(INFO) << "Index image " << ID;
}

void QueryImage(unsigned ID, vector<int>& query_IDs, cv::Mat& keys, cv::Mat& descs, vector<unsigned>& IDs, map<unsigned, unsigned>& indexIDs, 
			Eigen::Matrix<float, Eigen::Dynamic, 3, Eigen::RowMajor>& locationMatrix)
{
	query_IDs.clear();
	
	if (OPT::nQuery > 0 && keys.rows > 0 && descs.rows > 0)
	{
		LOG(INFO) << "Query image: " << ID;
		assert(keys.rows == descs.rows);
		int featureNum = keys.rows;

		colmap::FeatureKeypoints keypoints;
		keypoints.resize(featureNum);
		float* data = (float*)keys.data;
		for (int j = 0; j < featureNum; j++)
		{
			keypoints[j] = colmap::FeatureKeypoint(*data, *(data + 1), *(data + 2), *(data + 3));
			data += 8;
		}

		colmap::FeatureDescriptors descriptors(descs.rows, descs.cols);
		if (OPT::bFloatDesc)
		{
			Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> descriptorsFloat(descs.rows, descs.cols);
			cv::cv2eigen(descs, descriptorsFloat);
			descriptors = colmap::FeatureDescriptorsToUnsignedByte(descriptorsFloat);
		}
		else
			cv::cv2eigen(descs, descriptors);
		

		//LOG(INFO) << "Query image " << ID << ": ";
		colmap::retrieval::VisualIndex<>::QueryOptions queryOptions;
		queryOptions.max_num_images = OPT::nQuery + 1;
		vector<colmap::retrieval::ImageScore> imageScores;
		visualIndex.Query(queryOptions, keypoints, descriptors, &imageScores);
		for (const auto imageScore : imageScores)
		{
			if (ID == imageScore.image_id)
				continue;
			query_IDs.push_back(imageScore.image_id);
			//LOG(INFO) << "  " << imageScore.image_id << "," << imageScore.score;
		}
	}

	if (OPT::nOverlap > 0)
	{
		int imageNum = IDs.size();
		int i = indexIDs[ID];
		//LOG(INFO) << "Overlap image " << ID << ": ";
		for (int j = 1; j <= OPT::nOverlap; j++)
		{
			if (i + j >= imageNum)
				break;
			query_IDs.push_back(IDs[i + j]);
			//LOG(INFO) << "  " << IDs[i + j];
		}
	}

	if (OPT::nSpatial > 0)
	{
		int id1 = ID;
		int i = indexIDs[ID];

		if (locationMatrix(i, 0) == 0.0 && 	locationMatrix(i, 1) == 0.0 && locationMatrix(i, 2) == 0.0)
		{
			//LOG(INFO) << "Spatial image " << id1 << ": (no GPS, skip)";
			return;
		}
		
		//LOG(INFO) << "Spatial image " << id1 << ":";
		int num_spatial_tmp = 0;
		for (int j = 0; j < num_spatial_plus; ++j)
		{
			int id2 = IDs[index_matrix(i, j)];
			if (id1 == id2 || distance_matrix(i, j) == 0)
				continue;

			//LOG(INFO) << "  " << id2 << "," << distance_matrix(i, j);
			query_IDs.push_back(id2);

			num_spatial_tmp++;
			if (num_spatial_tmp >= OPT::nSpatial)
				break;
		}
	}

	if (OPT::fDisSpatial > 0)
	{
		int id1 = ID;
		int i = indexIDs[ID];
		double dis = OPT::fDisSpatial * OPT::fDisSpatial;

		if (locationMatrix(i, 0) == 0.0 && 	locationMatrix(i, 1) == 0.0 && locationMatrix(i, 2) == 0.0)
		{
			//LOG(INFO) << "Spatial image " << id1 << ": (no GPS, skip)";
			return;
		}
		
		//LOG(INFO) << "Spatial image " << id1 << ":";
		for (int j = 0; j < num_spatial_plus; ++j)
		{
			int id2 = IDs[index_matrix(i, j)];
			if (id1 == id2 || distance_matrix(i, j) > dis)
				continue;

			//LOG(INFO) << "  " << id2 << "," << distance_matrix(i, j);
			query_IDs.push_back(id2);
		}
	}
}

void GetExifInfo(string fileName, double& focalLength, double& longitude, double& latitude, double& altitude)
{
	colmap::Bitmap bitmap;
	if (bitmap.Read(fileName, false))
	{
		if (!bitmap.ExifFocalLength(&focalLength))
			focalLength = -1.0;

		if (!bitmap.ExifLongitude(&longitude) || !bitmap.ExifLatitude(&latitude) || !bitmap.ExifAltitude(&altitude))
		{
			longitude = DBL_MAX;
			latitude = DBL_MAX;
			altitude = DBL_MAX;
		}
	}
	else
	{
		focalLength = -1.0;
		longitude = DBL_MAX;
		latitude = DBL_MAX;
		altitude = DBL_MAX;
	}
}

void LBH2XYZ(double l, double b, double h, double &x, double &y, double &z)
{
	if (l == DBL_MAX || b == DBL_MAX)
	{
		x = DBL_MAX;
		y = DBL_MAX;
		z = DBL_MAX;
		return;
	}
	//坐标系/投影方式/EPSG代号:  Xian 1980 / 3-degree Gauss-Kruger zone 40  /EPSG:2364
	//投影方式适用范围:中国东经118°30'~东经121°30'
	//参数:	
	const char *city = "+proj=tmerc +lat_0=0 +lon_0=120 +k=1 +x_0=40500000 +y_0=0 +a=6378140 +b=6356755.288157528 +units=m +no_defs ";
	const char *wgs84 = "+proj=longlat +datum=WGS84 +no_defs "; //GPS所用坐标系,EPSG:4326
	projPJ pj_wgs84 = pj_init_plus(wgs84);
	projPJ pj_city = pj_init_plus(city);

	//经纬度角度改弧度
	x = l * DEG_TO_RAD;
	y = b * DEG_TO_RAD;

	//将GPS经纬度投影转换
	//在东经118°30'~东经121°30'进行投影计算
	pj_transform(pj_wgs84, pj_city, 1, 1, &x, &y, NULL);
	z = h;
}

void XYZ2LBH(double &l, double &b, double &h, double x, double y, double z)
{
	if (x == DBL_MAX || y == DBL_MAX)
	{
		l = DBL_MAX;
		b = DBL_MAX;
		h = DBL_MAX;
		return;
	}

	//坐标系/投影方式/EPSG代号:  Xian 1980 / 3-degree Gauss-Kruger zone 40  /EPSG:2364
	//投影方式适用范围:中国东经118°30'~东经121°30'
	//参数:	
	const char *city = "+proj=tmerc +lat_0=0 +lon_0=120 +k=1 +x_0=40500000 +y_0=0 +a=6378140 +b=6356755.288157528 +units=m +no_defs ";
	const char *wgs84 = "+proj=longlat +datum=WGS84 +no_defs "; //GPS所用坐标系,EPSG:4326
	projPJ pj_wgs84 = pj_init_plus(wgs84);
	projPJ pj_city = pj_init_plus(city);

	l = x;
	b = y;

	pj_transform(pj_city, pj_wgs84, 1, 1, &l, &b, NULL);

	l *= RAD_TO_DEG;
	b *= RAD_TO_DEG;
	h = z;

	if (isinf(l) || isnan(l) || isinf(b) || isnan(b))
	{
		l = DBL_MAX;
		b = DBL_MAX;
		h = DBL_MAX;
	}
}

// 根据GPS相似变换(ransac)
bool SimilarityTransformRansac(REC3D::ImageArr &images, REC3D::PointCloud &pointCloud)
{
	std::vector<Eigen::Vector3d> src_mat;
    std::vector<Eigen::Vector3d> dst_mat;
	for(REC3D::Image& image : images)
	{
		if (!image.valid || image.gpsLocal.x == DBL_MAX || image.gpsLocal.y == DBL_MAX || image.gpsLocal.z == DBL_MAX || cv::countNonZero(image.R) == 0)
			continue;
		
		Eigen::Vector3d srcC(image.C.x, image.C.y, image.C.z);
		Eigen::Vector3d dstC(image.gpsLocal.x, image.gpsLocal.y, image.gpsLocal.z);
		src_mat.push_back(srcC);
        dst_mat.push_back(dstC);
	}

	int num = dst_mat.size();
	LOG(INFO) << "Found " << num << " geotagged images";
	if (num < 3)
		return 0;
	
	colmap::RANSACOptions options;
    options.max_error = OPT::fGPSErr;
    options.min_inlier_ratio = 0.01;
	colmap::LORANSAC<colmap::SimilarityTransformEstimator<3>, colmap::SimilarityTransformEstimator<3>> ransac(options);
    const auto report = ransac.Estimate(src_mat, dst_mat);

	if (report.success)
		LOG(INFO) << "Similarity transformation inliers:" << report.support.num_inliers << ", trials:" << report.num_trials;
	else
	{
		LOG(ERROR) << "Similarity transformation failed, max " << report.support.num_inliers << " inliers";
		return 0;
	}

	Eigen::Matrix<double, 3, 4> model34 = report.model;
	Eigen::Matrix<double, 3, 3> model33 = model34.topLeftCorner(3, 3);
	cv::Matx33d transR;
	double scale = model33.col(0).norm();
	cv::eigen2cv(model33, transR);
	cv::Point3d transT;
	transT.x = model34(0, 3);
	transT.y = model34(1, 3);
	transT.z = model34(2, 3);
	LOG(INFO) << "Scale: " << scale;
	LOG(INFO) << "Rotation: " << transR.row(0);
	LOG(INFO) << "          " << transR.row(1);
	LOG(INFO) << "          " << transR.row(2);
	LOG(INFO) << "Translation: " << transT;
	
	std::vector<double> residuals;
	double meanResidual = 0;
	for(int i=0; i< num; i++)
	{
		Eigen::Vector3d srcPt = src_mat[i];
		Eigen::Vector3d dstTrans = model34 * srcPt.homogeneous();
		double residual = (dst_mat[i] - dstTrans).norm();
		residuals.push_back(residual);
		meanResidual += residual / num;
	}
	std::sort(residuals.begin(), residuals.end());
	LOG(INFO) << "Mean residual: " << meanResidual << ", median residual: " << residuals[residuals.size() / 2] << ", max residual: " << residuals[residuals.size() - 1];
	
	for(REC3D::Image &image : images)
	{
		if (!image.valid || cv::countNonZero(image.R) == 0)
			continue;
		
		cv::Point3d C = image.C;
		image.C = transR * C + transT;
		cv::Matx33d R = image.R;
		image.R = 1 / scale * R * transR.t();
	}
	LOG(INFO) << "Camera coordinate transformation done";

	for (cv::Point3f &pt : pointCloud.points)
	{
		cv::Point3d ptd = pt;
		pt = transR * ptd + transT;
	}
	LOG(INFO) << "Points coordinate transformation done";

	return 1;
} // 根据GPS相似变换(ransac)